#include "mpu6050.h"
#include "mpu6xxx_reg.h"
#include "Fusion.h"
#include "time.h"

#define MPU6050_DEVICE_NAME  "i2c1"
#define SAMPLE_PERIOD (0.01f)
#define SAMPLE_RATE (1/SAMPLE_PERIOD)
static rt_thread_t mpu6050_tid = RT_NULL;
static rt_timer_t mpu6050_timer;
static struct mpu6xxx_device *mpu6050_dev;
int pitch,roll,yaw;



//定时恢复mpu6050数据采集线程
static void mpu6050_timeout(void *p)
{
    //恢复mpu6050数据采集线程并立马切换线程
    rt_thread_resume(mpu6050_tid);
    rt_schedule();

}

//mpu6050原始数据获得+解算数据
static void mpu6050_entry(void *p)
{
    struct mpu6xxx_3axes mpu6050_accel, mpu6050_gyro,mpu6050_mag;
    FusionVector gyroscope,accelerometer,magnetometer;

//    // Define calibration (replace with actual calibration data if available)
//    const FusionMatrix gyroscopeMisalignment = {1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f};
//    const FusionVector gyroscopeSensitivity = {1.0f, 1.0f, 1.0f};
//    const FusionVector gyroscopeOffset = {0.0f, 0.0f, 0.0f};
//    const FusionMatrix accelerometerMisalignment = {1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f};
//    const FusionVector accelerometerSensitivity = {1.0f, 1.0f, 1.0f};
//    const FusionVector accelerometerOffset = {0.0f, 0.0f, 0.0f};
//    const FusionMatrix softIronMatrix = {1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f};
//    const FusionVector hardIronOffset = {0.0f, 0.0f, 0.0f};
//
//    // Initialise algorithms
//    FusionOffset offset;
//    FusionAhrs ahrs;
//
//    FusionOffsetInitialise(&offset, SAMPLE_RATE);
//    FusionAhrsInitialise(&ahrs);
//
//    // Set AHRS algorithm settings
//      const FusionAhrsSettings settings = {
//              .convention = FusionConventionNwu,
//              .gain = 0.5f,
//              .gyroscopeRange = 2000.0f, /* replace this with actual gyroscope range in degrees/s */
//              .accelerationRejection = 10.0f,
//              .magneticRejection = 10.0f,
//              .recoveryTriggerPeriod = 5 * SAMPLE_RATE, /* 5 seconds */
//      };
//      FusionAhrsSetSettings(&ahrs, &settings);
    FusionAhrs ahrs;
    FusionAhrsInitialise(&ahrs);

    while(1)
    {

        //获取数据
        //rt_kprintf("s:%d\n",rt_tick_get());
        mpu6xxx_get_gyro(mpu6050_dev, &mpu6050_gyro);
        mpu6xxx_get_accel(mpu6050_dev, &mpu6050_accel);
        mpu6xxx_get_mag(mpu6050_dev, &mpu6050_mag);

        //计算真实值
        gyroscope.array[0] = (float)(mpu6050_gyro.x) / 16.384;
        gyroscope.array[1] = (float)(mpu6050_gyro.y) / 16.384;
        gyroscope.array[2] = (float)(mpu6050_gyro.z) / 16.384;
        accelerometer.array[0] = (float)(mpu6050_accel.x) / 2048;
        accelerometer.array[1] = (float)(mpu6050_accel.y) / 2048;
        accelerometer.array[2] = (float)(mpu6050_accel.z) / 2048;
        magnetometer.array[0] = (float)(mpu6050_mag.x);
        magnetometer.array[1] = (float)(mpu6050_mag.y);
        magnetometer.array[2] = (float)(mpu6050_mag.z);
        //解算姿态



//        // Acquire latest sensor data
//        const clock_t timestamp = rt_tick_get(); // replace this with actual gyroscope timestamp
//
//        // Apply calibration
//        gyroscope = FusionCalibrationInertial(gyroscope, gyroscopeMisalignment, gyroscopeSensitivity, gyroscopeOffset);
//        accelerometer = FusionCalibrationInertial(accelerometer, accelerometerMisalignment, accelerometerSensitivity, accelerometerOffset);
//        magnetometer = FusionCalibrationMagnetic(magnetometer, softIronMatrix, hardIronOffset);
//
//        // Update gyroscope offset correction algorithm
//        gyroscope = FusionOffsetUpdate(&offset, gyroscope);
//
//        // Calculate delta time (in seconds) to account for gyroscope sample clock error
//        static clock_t previousTimestamp;
//        const float deltaTime = (float) (timestamp - previousTimestamp) / (float) CLOCKS_PER_SEC;
//        previousTimestamp = timestamp;
//
//        // Update gyroscope AHRS algorithm
//        FusionAhrsUpdate(&ahrs, gyroscope, accelerometer, magnetometer, deltaTime);
//
//        // Print algorithm outputs
//        const FusionEuler euler = FusionQuaternionToEuler(FusionAhrsGetQuaternion(&ahrs));
//        //const FusionVector earth = FusionAhrsGetEarthAcceleration(&ahrs);


        FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, SAMPLE_PERIOD);

        const FusionEuler euler = FusionQuaternionToEuler(FusionAhrsGetQuaternion(&ahrs));


        pitch = euler.angle.pitch*100;
        roll = euler.angle.roll*100;
        yaw = euler.angle.yaw*100;
        //rt_kprintf("e:%d\n",rt_tick_get());
        rt_kprintf("%d,%d,%d\r\n", pitch,roll,yaw);

        //挂起当前线程并立马进行上下文切换
        rt_thread_suspend(mpu6050_tid);
        rt_schedule();
    }
}

//外部调用初始化mpu6050
int mpu6050_thread_init(void)
{
    //初始化mpu6050
    mpu6050_dev = mpu6xxx_init(MPU6050_DEVICE_NAME, RT_NULL);
    mpu6xxx_set_param(mpu6050_dev, MPU6XXX_GYRO_RANGE, MPU6XXX_GYRO_RANGE_2000DPS);
    mpu6xxx_set_param(mpu6050_dev, MPU6XXX_ACCEL_RANGE, MPU6XXX_ACCEL_RANGE_16G);
    if (mpu6050_dev == RT_NULL)
    {
        rt_kprintf("mpu6050 init failed\n");
        return -1;
    }
    rt_kprintf("mpu6050 init succeed\n");


    //创建读取数据线程
    mpu6050_tid = rt_thread_create("mpu 6050", mpu6050_entry, RT_NULL, 1024, 1, 20);
    if (mpu6050_tid != RT_NULL) {
            rt_kprintf("mpu6050_tid init\r\n");
            rt_thread_startup(mpu6050_tid);
        }
        else{
            rt_kprintf("mpu6050_tid = RT_NULL\r\n");
            return RT_ERROR;
        }

    //创建定时器周期处理数据
    mpu6050_timer =  rt_timer_create("mpu6050 timer", mpu6050_timeout,
                                    RT_NULL, SAMPLE_PERIOD*1000,
                                    RT_TIMER_FLAG_PERIODIC);
    if (mpu6050_timer != RT_NULL) {
        rt_timer_start(mpu6050_timer);
        rt_kprintf("mpu6050 timer start\r\n");
    }else{
        rt_kprintf("mpu6050 timer = RT_NULL\r\n");
        return RT_ERROR;
    }

    return 0;
}
